#!/usr/bin/env python
#-*- coding:UTF-8 -*-
import rospy 

uav_dir = "/home/guanty18/catkin_ws/src/uav"

if __name__ == "__main__":

    rospy.init_node('multi_gen', anonymous=True)

    uav_number = rospy.get_param("uav_number",3)
    x = []
    y = []
    for n in range(uav_number):
        x.append(rospy.get_param("x"+str(n),0))
        y.append(rospy.get_param("y"+str(n),0))

    muavlines =[]
    with open(uav_dir+"/launch/template.launch",'r') as f:
        lines = f.readlines()
        headlines = lines[:23]
        uavlines_tmp = lines[23:52]
        endlines = lines[52:]
        for n in range(uav_number):
            for line in uavlines_tmp:
                muavlines.append(line)
        for n in range(uav_number):
            muavlines[0+n*29] = muavlines[0+n*29].replace("UAV","UAV"+str(n))
            muavlines[1+n*29] = muavlines[1+n*29].replace('uav','uav'+str(n))
            muavlines[3+n*29] = muavlines[3+n*29].replace('0',str(n))
            muavlines[4+n*29] = muavlines[4+n*29].replace('14540',str(14540+n))
            muavlines[4+n*29] = muavlines[4+n*29].replace('14580',str(14580+n))
            muavlines[7+n*29] = muavlines[7+n*29].replace('0',str(x[n]))
            muavlines[8+n*29] = muavlines[8+n*29].replace('0',str(y[n]))
            muavlines[14+n*29] = muavlines[14+n*29].replace('14560',str(14560+n))
            muavlines[15+n*29] = muavlines[15+n*29].replace('4560',str(4560+n))
            muavlines[17+n*29] = muavlines[17+n*29].replace('5600',str(5600+n))
            muavlines[25+n*29] = muavlines[25+n*29].replace('1',str(1+n))

        with open(uav_dir+"/launch/multi_uav.launch",'w') as r:
            for line in headlines:
                r.write(line)
            for line in muavlines:
                r.write(line)
            for line in endlines:
                r.write(line)


